# -*- encoding: UTF-8 -*-

'''Walk: Small example to make Nao walk'''
''' This example is only compatible with NAO '''

import argparse
import motion
import time
import almath
from naoqi import ALProxy

IP = "192.168.1.103"
PORT = 9559

def footmove(motionProxy):
    x = 1.0
    y = 1.0
    theta = 0.0
    frequency = 0.5
    motionProxy.moveToward(x, y, theta, [["Frequency", frequency]])
    # Lets make him stop after 3 seconds

    motionProxy.stopMove()


def userArmArticular1(motionProxy):
    # Arms motion from user have always the priority than walk arms motion
    JointNames = ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand"]
    Arm1 = [76.5, 35.0, -63.5, -59.6, -39.3, 0]
    Arm1 = [x * motion.TO_RAD for x in Arm1]

    pFractionMaxSpeed = 0.1

    motionProxy.angleInterpolationWithSpeed(JointNames, Arm1, pFractionMaxSpeed)


def userArmArticular2(motionProxy):
    # Arms motion from user have always the priority than walk arms motion
    JointNames = ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand"]
    Arm2 = [70.5, 25.0, -63.5, -59.6, -39.3, 1]
    Arm2 = [x * motion.TO_RAD for x in Arm2]

    pFractionMaxSpeed = 0.1

    motionProxy.angleInterpolationWithSpeed(JointNames, Arm2, pFractionMaxSpeed)


def userArmArticular3(motionProxy):
    # Arms motion from user have always the priority than walk arms motion
    JointNames = ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand"]
    Arm3 = [67.8, 19, -62.8, -58.6, -24, 1]
    Arm3 = [x * motion.TO_RAD for x in Arm3]

    pFractionMaxSpeed = 0.1

    motionProxy.angleInterpolationWithSpeed(JointNames, Arm3, pFractionMaxSpeed)


def userArmArticular4(motionProxy):
    # Arms motion from user have always the priority than walk arms motion
    JointNames = ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand"]
    Arm3 = [67.2, 4.9, -57.8, -58.6, -24, 1]
    Arm3 = [x * motion.TO_RAD for x in Arm3]

    pFractionMaxSpeed = 0.1

    motionProxy.angleInterpolationWithSpeed(JointNames, Arm3, pFractionMaxSpeed)


def main():
    motionProxy = ALProxy("ALMotion", IP, PORT)
    postureProxy = ALProxy("ALRobotPosture", IP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand
    postureProxy.goToPosture("StandInit", 0.5)

    #####################
    ## Enable arms control by Motion algorithm
    #####################
    motionProxy.setMoveArmsEnabled(True, True)
    # motionProxy.setMoveArmsEnabled(False, False)
    footmove(motionProxy)
    time.sleep(3.0)
    #####################
    ## FOOT CONTACT PROTECTION
    #####################
    # motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", False]])
    userArmArticular1(motionProxy)
    time.sleep(1.0)

    userArmArticular2(motionProxy)
    time.sleep(1.0)
    motionProxy.openHand('LHand')
    motionProxy.closeHand('LHand')

    #####################
    ## End Walk
    #####################
    # TARGET VELOCITY
    userArmArticular3(motionProxy)
    time.sleep(1.0)
    userArmArticular4(motionProxy)
    time.sleep(1.0)

    motionProxy.waitUntilMoveIsFinished()

    motionProxy.rest()
    # Go to rest position


if __name__ == "__main__":
    # parser = argparse.ArgumentParser()
    # parser.add_argument("--ip", type=str, default="192.168.1.103",
    #                     help="Robot ip address")
    # parser.add_argument("--port", type=int, default=9559,
    #                     help="Robot port number")
    #
    # args = parser.parse_args()
    main()
